#include <sonar/srf04.h>

#include <arduino/Arduino.h>


namespace sonar
{


void srf04::setup(int pinIn, int pinOut)
{
  _pin_in = pinIn;
  _pin_out = pinOut;

  pinMode(_pin_out, OUTPUT);
  pinMode(_pin_in, INPUT);
}


void srf04::loop(void)
{
  // Make sure pin is low before sending a short high to trigger ranging
  digitalWrite(_pin_out, LOW);

  delayMicroseconds(2);

  // Send a short 10 microsecond high burst on pin to start ranging
  digitalWrite(_pin_out, HIGH);

  delayMicroseconds(10);

  // Send pin low again before waiting for pulse back in
  digitalWrite(_pin_out, LOW);

  // Reads echo pulse in from SRF04 in micro seconds
  _duration = pulseIn(_pin_in, HIGH);

  // Dividing this by 58 gives us a distance in cm
  _measure = _duration/58;
}


} // sonar
